#include "skeletonLoader.h"

using namespace std;

SkeletonLoader::SkeletonLoader() {
}

SkeletonLoader::~SkeletonLoader() {
}

Skeleton* SkeletonLoader::load(string filename) {
   FileType id = noneFile;
   if (filename.substr(filename.size() - 4, 4) == ".asf") {
      id = asfFile;
   } else if (filename.substr(filename.size() - 4, 4) == ".bvh") {
      id = bvhFile;
   }
   if (id == noneFile) {
      cout << "File format not recognizeg!" << endl;
      return 0;
   } else {
      return load(filename, id);
   }
}

Skeleton* SkeletonLoader::load(string filename, FileType id) {
   switch (id) {
      case asfFile:
         return loadASF(filename);
         break;
      case bvhFile:
         return loadBVH(filename);
         break;
      default:
         break;
   }
   return 0;
}

Skeleton* SkeletonLoader::loadASF(string filename) {
   
   Skeleton *s = new Skeleton();
   Bone *b; // for bones reading

   ifstream file(filename.c_str());

   string line, current_command;
   vector<string> words;
   
   words.clear();
   
   bool is_degree = false; // for identification of angles unit
   int current_limit = 0;   

   if (file.is_open()) {
      file >> noskipws;
      while (!file.eof()) {
         getline(file, line);
         words = readWords(line);         
         if (line[0] == ':') {
            current_command = words[0];
         } else if (line[0] == '#' || line == "") {
            continue; // comentary line or end line
         }
         
         if (current_command == ":version") {
            // Skeleton Version
         } else if(current_command == ":name"){
            // Skeleton Name
         } else if(current_command == ":units"){
            words = readWords(line);
            if (words[0] == "mass") { // mass
               // Mass Unit
            } else if (words[0] == "length") { // length
               // Length Unit
            } else if (words[0] == "angle") { // angle
               is_degree = words[1] == "deg" ? true : false;
            }
         } else if(current_command == ":documentation"){
            // Documentation
         } else if(current_command == ":root"){
            if (words[0] == "order") { // order
               string order = "";
               for (unsigned int i = 1; i < words.size(); i++){
                  if (words[i][0] == 'T' || words[i][0] == 't') {
                     switch (words[i][1]) {
                        case 'x':
                        case 'X':
                           order += 'x';
                           break;
                        case 'y':
                        case 'Y':
                           order += 'y';
                           break;
                        case 'z':
                        case 'Z':
                           order += 'z';
                           break;
                        default:
                           break;
                     }
                  } else if (words[i][0] == 'R' || words[i][0] == 'r') {
                     switch (words[i][1]) {
                        case 'x':
                        case 'X':
                           order += 'X';
                           break;
                        case 'y':
                        case 'Y':
                           order += 'Y';
                           break;
                        case 'z':
                        case 'Z':
                           order += 'Z';
                           break;
                        default:
                           break;
                     }
                  }
               }
               s->setTrOrder(order);
            } else if (words[0] == "axis") { // axis
               // Root Axis
            } else if (words[0] == "position") { // position
               Number x = strtod(words[1].c_str(), NULL),
                      y = strtod(words[2].c_str(), NULL),
                      z = strtod(words[3].c_str(), NULL);
               Point3D root_position;
               // TODO: Implents acess with x, y and z on vector class and point class
               s->setRootPosition(Point3D(x, y, z));
            } else if (words[0] == "orientation") { // orientation               
               Number x = strtod(words[1].c_str(), NULL),
                      y = strtod(words[2].c_str(), NULL),
                      z = strtod(words[3].c_str(), NULL);
               Vector3D orientation(x, y, z);
            }
         } else if(current_command == ":bonedata"){
            if (words[0] == "begin") { // begin [reseting values]
               b = new Bone();
               current_limit = 0;
            } else if (words[0] == "id") { // read bone id
               // Bone id
            } else if (words[0] == "name") {
               string name = words[1];
               b->setName(name);
            } else if (words[0] == "direction") { // read bone direction
               Number x = strtod(words[1].c_str(), NULL),
                      y = strtod(words[2].c_str(), NULL),
                      z = strtod(words[3].c_str(), NULL);
               Vector3D direction(x, y, z);
               b->setDirection(direction);
            } else if (words[0] == "length") { // read bone length
               double length = strtod(words[1].c_str(), NULL);
               b->setLength(length);
            } else if (words[0] == "axis") { // read bone axis               
               Number x = strtod(words[1].c_str(), NULL),
                      y = strtod(words[2].c_str(), NULL),
                      z = strtod(words[3].c_str(), NULL);
               Vector3D axisRot(x, y, z);
               string axis_order = words[4];
               b->setAxisRot(axisRot, axis_order, is_degree);
            } else if (words[0] == "bodymass") { // read bone bodymass
               // Bone Bodymass
            } else if (words[0] == "cofmass") { // read bone cofmass
               // Bone Cofmass
            } else if (words[0] == "dof") { // read bone dof
               string dof = "";
               for (unsigned int i = 1; i < words.size(); i++){
                  if (words[i][0] == 'r') {
                     switch (words[i][1]) {
                        case 'x':
                        case 'X':
                           dof += 'X';
                           break;
                        case 'y':
                        case 'Y':
                           dof += 'Y';
                           break;
                        case 'z':
                        case 'Z':
                           dof += 'Z';
                           break;
                        default:
                           break;
                     }
                  } else {
                     switch (words[i][1]) {
                        case 'x':
                        case 'X':
                           dof += 'x';
                           break;
                        case 'y':
                        case 'Y':
                           dof += 'y';
                           break;
                        case 'z':
                        case 'Z':
                           dof += 'z';
                           break;
                        default:
                           break;
                     }
                  }
               }
               current_limit = words.size() - 2;
               b->setAxisOrder(dof);
            } else if (words[0] == "limits" && current_limit> 0) { // read bone limits
               Number x = strtod(words[words.size() - 2].substr(1).c_str(), NULL),
                      y = strtod(words[words.size() - 1].substr(0, words[words.size() - 1].size() - 1).c_str(), NULL);
               Vector2D limit(x, y);
               current_limit -= 1;
            } else if (words[0] == "end") { // end [inserting on map]
               s->insertBone(b);
            }
         } else if(current_command == ":hierarchy"){
            if (words[0] == "begin") { // begin [reseting values]
               // Begin Hierarchy
            } else if (words[0] == "end") { // end [inserting on map]
               // End Hierarchy
            } else if (words[0][0] != ':') { // insert one dependence on map
               string bone = words[0];
               for (unsigned int i = 1; i < words.size(); i++){
                  s->insertSon(bone, words[i]);                  
               }
            }
         } else {
            cout << "Error on file. Check the asf format file specification" << endl;
            return 0;
         }
      }      
      file.close();
   } else {
      cout << "Problem on open file" << endl;
      return 0;
   }
   return s;
}

Skeleton* SkeletonLoader::loadBVH(string filename) {
   
   Skeleton *s = new Skeleton();
   Bone *b; // for bones reading

   ifstream file(filename.c_str());

   string line, current_bone;
   vector<string> words;
   list<string> boneQueue;
   bool isFirst = true;

   words.clear();
   boneQueue.clear();

   if (file.is_open()) {
      file >> noskipws;

      getline(file, line);
      words = readWords(line);
      if (words[0] != "HIERARCHY") {
         loadBvhFail();
         return 0;
      }

      while (!file.eof()) {
         getline(file, line);
         words = readWords(line);
         if (words[0] == "ROOT") {
            current_bone = "root";                       

            getline(file, line);
            words = readWords(line);
            if (words[0] != "{") {
               loadBvhFail();
               return 0;
            }
            
            boneQueue.push_front(current_bone);

            getline(file, line);
            words = readWords(line);
            if (words[0] != "OFFSET") {
               loadBvhFail();
               return 0;
            }

            Number x = strtod(words[1].c_str(), NULL),
                   y = strtod(words[2].c_str(), NULL),
                   z = strtod(words[3].c_str(), NULL);
            Point3D root_position(x, y, z);

            getline(file, line);
            words = readWords(line);
            if (words[0] != "CHANNELS") {
               loadBvhFail();
               return 0;
            }

            string order = "";
            for (unsigned int i = 2; i < words.size(); i++) {
               if (words[i].substr(1) == "position") {
                  switch (words[i][0]) {
                     case 'x':
                     case 'X':
                        order += 'x';
                        break;
                     case 'y':
                     case 'Y':
                        order += 'y';
                        break;
                     case 'z':
                     case 'Z':
                        order += 'z';
                        break;
                     default:
                        break;
                  }
               } else if (words[i].substr(1) == "rotation") {
                  switch (words[i][0]) {
                     case 'x':
                     case 'X':
                        order += 'X';
                        break;
                     case 'y':
                     case 'Y':
                        order += 'Y';
                        break;
                     case 'z':
                     case 'Z':
                        order += 'Z';
                        break;
                     default:
                        break;
                  }
               } else {
                  loadBvhFail();
                  return 0;
               }
            }
            
            s->setRootPosition(root_position);
            s->setTrOrder(order);
            
         } else if (words[0] == "JOINT") {

            current_bone = words[1];
            s->insertSon(boneQueue.front(), current_bone);
            
            if(!isFirst){ // if the first iteration, don't have bone to insert
               s->insertBone(b);
            }
            
            isFirst = false;
            b = new Bone();
            b->setName(current_bone);
            
         } else if (words[0] == "End") {

            current_bone = words[0]+boneQueue.front();
            s->insertSon(boneQueue.front(), current_bone);
            s->insertBone(b);

            b = new Bone();
            b->setName(current_bone);

         } else if (words[0] == "CHANNELS") {

            string order = "";
            for (unsigned int i = 2; i < words.size(); i++) {
               if (words[i].substr(1) == "position") {
                  switch (words[i][0]) {
                     case 'x':
                     case 'X':
                        order += 'x';
                        break;
                     case 'y':
                     case 'Y':
                        order += 'y';
                        break;
                     case 'z':
                     case 'Z':
                        order += 'z';
                        break;
                     default:
                        break;
                  }
               } else if (words[i].substr(1) == "rotation") {
                  switch (words[i][0]) {
                     case 'x':
                     case 'X':
                        order += 'X';
                        break;
                     case 'y':
                     case 'Y':
                        order += 'Y';
                        break;
                     case 'z':
                     case 'Z':
                        order += 'Z';
                        break;
                     default:
                        break;
                  }
               } else {
                  loadBvhFail();
                  return 0;
               }
            }
            
            b->setAxisOrder(order);

         } else if (words[0] == "OFFSET") {
            
            Number x = strtod(words[1].c_str(), NULL),
                   y = strtod(words[2].c_str(), NULL),
                   z = strtod(words[3].c_str(), NULL);            
            Vector3D offset(x, y, z);
            b->setDirection(normalize(offset));
            b->setLength(length(offset));
                        
         } else if (words[0] == "{") {
            boneQueue.push_front(current_bone);
         } else if (words[0] == "}") {
            boneQueue.pop_front();            
         } else if (words[0] == "MOTION") {
            file.close();
            break;
         }
      }      
   } else {
      cout << "Problem on open file" << endl;
      return 0;
   }
   return s;
}
